// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ParticleFilterTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using MicrosoftResearch.Infer.Distributions;

    /// <summary>
    /// This test simulates a robot that operates in a one-dimensional space.
    /// The robot starts in the origin of the X axis. There is an obstacle at 5000 units away to the right.
    /// The robot is commanded to travel 1 unit for 5000 iterations. It actually travels anywhere between approx. 1 and 2 units (with noise given by a gaussian of variance MotionModelVariance, taken in absolute value).
    /// The robot is guaranteed to travel more than 1, just to make sure the noise does not average to 0 over time.
    /// The robot observes the remaining distance to the obstacle. The observation has noise anywhere between approx. -2 and 2 units (with noise given by a gaussian of variance SensorModelVariance).
    /// At every step, the robot executes the motion command and observes the distance remaining. The motion command and the observation are fed into the particle filter.
    /// </summary>
    [TestClass]
    public class ParticleFilterTest : ParticleFilter<double, double, double>
    {
        /// <summary>
        /// The sensor model variance (gaussian with 0 mean).
        /// </summary>
        private const double SensorModelVariance = 1;

        /// <summary>
        /// The motion model variance (gaussian with 0 mean)
        /// </summary>
        private const double MotionModelVariance = 0.5;

        /// <summary>
        /// Location of the target, as a positive number
        /// this value also dictates the number of iterations, as the simulated robot moves approx. 1 step at a time
        /// </summary>
        private int targetPosition;

        /// <summary>
        /// Use simple gaussian noise for the sensor model
        /// </summary>
        private Gaussian sensorModel = new Gaussian(0, SensorModelVariance);

        /// <summary>
        /// Use simple gaussian noise for the motion model
        /// </summary>
        private Gaussian motionModel = new Gaussian(0, MotionModelVariance);

        /// <summary>
        /// Use a constant distribution for the random state generator
        /// </summary>
        private Random random = new Random();

        /// <summary>
        /// Dictates the interval from which to sample the particles
        /// </summary>
        private bool isMultimodal;

        /// <summary>
        /// Initializes a new instance of the ParticleFilterTest class.
        /// </summary>
        public ParticleFilterTest()
            : base(1000, "ParticleFilterTest")
        {
        }

        /// <summary>
        /// This test simulates a robot that operates in a one-dimensional space. 
        /// The probability distribution is unimodal because the robot could only be on the left side of the target.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void UnimodalParticleFilterTest()
        {
            // reset the particles
            this.ResampleAll();

            // disable random resampling since this is approximating a unimodal distribution
            this.EnableRandomResampling = false;

            // since the position dictates the number of iterations, make this large so that we see the particle filter converging.
            this.targetPosition = 5000;

            // the state space is [0, TargetPosition], meaning the robot can only be on the left side of the target
            this.isMultimodal = false;

            // run the simulation
            int totalSteps = this.targetPosition / 2;
            double distanceTraveled = this.RunSimulation(0, totalSteps);

            // our particles should be clustered around the two true states
            GaussianEstimator clusterDistribution = new GaussianEstimator();
            foreach (double particle in this.Particles)
            {
                clusterDistribution.Add(particle);
            }

            // we expect a precise approximation of the true position
            // mean within 0.1% of the distance traveled, variance of 1% of actual distance traveled
            double expectedMeanError = distanceTraveled / 1000;
            double expectedVariance = distanceTraveled / 100;
            this.ValidateGaussianDistribution(
                clusterDistribution, distanceTraveled, expectedMeanError, expectedVariance, "Unimodal cluster");
        }

        /// <summary>
        /// This test simulates a robot that operates in a one-dimensional space. 
        /// The probability distribution is bimodal because the robot could be on either side of the target.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [Ignore]
        public void MultimodalParticleFilterTest()
        {
            // reset the particles
            this.ResampleAll();

            // enable random resampling since this is approximating a multimodal distribution
            this.EnableRandomResampling = true;

            // since the particle filter becomes unimodal after many iterations, here we need to keep the number of iterations small
            this.targetPosition = 500;

            // the state space is [0, 2*TargetPosition], meaning the robot could be on either side of the target
            this.isMultimodal = true;

            // run the simulation
            int totalSteps = this.targetPosition / 2;
            double distanceTraveled = this.RunSimulation(0, totalSteps);

            // our particles should be clustered around the two true states
            GaussianEstimator leftClusterDistribution = new GaussianEstimator();
            GaussianEstimator rightClusterDistribution = new GaussianEstimator();
            foreach (double particle in this.Particles)
            {
                if (particle <= this.targetPosition)
                {
                    leftClusterDistribution.Add(particle);
                }
                else
                {
                    rightClusterDistribution.Add(particle);
                }
            }

            // once completed, we expect a less precise approximation than in the unimodal case, since we ran a smaller number of iterations
            // mean within 1% of the distance traveled, variance of 5% of actual distance traveled
            double expectedMeanError = distanceTraveled / 100;
            double expectedVariance = distanceTraveled / 20;

            this.ValidateGaussianDistribution(
                leftClusterDistribution, 
                distanceTraveled, 
                expectedMeanError, 
                expectedVariance, 
                "Multimodal, left cluster");
            this.ValidateGaussianDistribution(
                rightClusterDistribution, 
                (2 * this.targetPosition) - distanceTraveled, 
                expectedMeanError, 
                expectedVariance, 
                "Multimodal, right cluster");
        }

        /// <summary>
        /// Clones an existing sample.
        /// </summary>
        /// <param name="sourceParticle">The existing particle.</param>
        /// <returns>A clone of the existing particle.</returns>
        protected override double CloneSample(double sourceParticle)
        {
            // return a clone of the state. In this case, a no-op.
            return sourceParticle;
        }

        /// <summary>
        /// Returns a sample from the state space that matches the observation.
        /// The default behavior is to ignore the observation and return a random sample.
        /// </summary>
        /// <param name="priorState">The old particle state.</param>
        /// <param name="update">The update to apply.</param>
        /// <param name="observation">The observation to match.</param>
        /// <param name="weight">The current weight of the particle</param>
        /// <param name="fitnessToObservation">A log probability value indicating the likelihood that the observation matches the new state</param>
        /// <returns>A sample from the state space.</returns>
        protected override double CreateRandomSampleFromPriorAndObservation(
            double priorState, double update, double observation, double weight, out double fitnessToObservation)
        {
            Assert.IsTrue(
                this.isMultimodal, 
                "This function should only be called for the multi-modal case, where EnableRandomResampling is true.");

            double candidatePosition = (this.isMultimodal && this.random.NextDouble() < 0.5)
                                           ? this.targetPosition + observation
                                           : this.targetPosition - observation;
            double newState = candidatePosition + (this.sensorModel.Sample() * 10);
                
                // inflate the error since we want a large variance of samples
            fitnessToObservation = this.sensorModel.GetLogProb(Math.Abs(this.targetPosition - newState) - observation);
            return newState;
        }

        /// <summary>
        /// Updates the given particle based on motion and observation and returns the probability that the state specified by the particle would result in the given observation.
        /// </summary>
        /// <param name="priorState">The old particle state.</param>
        /// <param name="update">The update to apply.</param>
        /// <param name="observation">The observation, assumed noisy.</param>
        /// <param name="weight">The current weight of the particle</param>
        /// <param name="fitnessToObservation">The probability of a fit, in log-likelihood form.</param>
        /// <returns>The updated particle.</returns>
        protected override double UpdateSample(
            double priorState, double update, double observation, double weight, out double fitnessToObservation)
        {
            double newParticleState = 0;
            double expectedObservation = 0;
            if (priorState <= this.targetPosition)
            {
                newParticleState = priorState + update;
                expectedObservation = this.targetPosition - newParticleState;
            }
            else
            {
                newParticleState = priorState - update;
                expectedObservation = newParticleState - this.targetPosition;
            }

            // add noise
            newParticleState = newParticleState + this.motionModel.Sample();
            expectedObservation = expectedObservation + (this.sensorModel.Sample() * 10);
                
                // inflate the error since we want a large variance of samples
            fitnessToObservation = this.sensorModel.GetLogProb(observation - expectedObservation);
            return newParticleState;
        }

        /// <summary>
        /// Helper which validates that a give distribution correctly approximates a particular position
        /// </summary>
        /// <param name="cluster">A gaussian cluster.</param>
        /// <param name="actualPosition">The actual position to compare against.</param>
        /// <param name="acceptableMeanError">The function asserts that the mean is within this error margin.</param>
        /// <param name="acceptableVariance">The function asserts that the variance is less than this value.</param>
        /// <param name="distributionName">The name of the distribution, for logging/error purposes. </param>
        /// <returns>The gaussian approximation of the cluster.</returns>
        private Gaussian ValidateGaussianDistribution(
            GaussianEstimator cluster, 
            double actualPosition, 
            double acceptableMeanError, 
            double acceptableVariance, 
            string distributionName)
        {
            Gaussian distribution = new Gaussian();
            distribution = cluster.GetDistribution(distribution);
            double mean = distribution.GetMean();
            double variance = distribution.GetVariance();

            Assert.IsTrue(
                Math.Abs(mean - actualPosition) < acceptableMeanError, 
                string.Format(
                    "{0}: mean error = {1}, expected {2}, pc={3}", 
                    distributionName, 
                    mean - actualPosition, 
                    acceptableMeanError, 
                    cluster.mva.Count));
            Assert.IsTrue(
                variance < acceptableVariance, 
                string.Format("{0}: variance = {1}, pc={2}", distributionName, variance, cluster.mva.Count));

            return distribution;
        }

        /// <summary>
        /// This test simulates a robot that operates in a one-dimensional space. 
        /// Whether the probability distribution is unimodal or bimodal (the robot could be on either side of the target) is decided outside this method.
        /// The robot starts at position 0 on the 1-dimensional map (axis) and advances in small increments towards an observable target.
        /// At every step, the distance to the target is observed, with noise.
        /// </summary>
        /// <param name="currentPosition">The robot position at the start of the simulation.</param>
        /// <param name="iterations">The number of iterations to run the filter. Each iteration moves the robot an average of 1.45 steps.</param>
        /// <returns>The end position of the robot (currentPosition + distanceTraveled).</returns>
        private double RunSimulation(double currentPosition, int iterations)
        {
            double distanceTraveled = currentPosition;

            // run some iterations of the filter, moving closer to the target and observing the distance to the target with noise
            for (int i = 0; i < iterations; i++)
            {
                // motion update of 1 + positive noise
                // this results in a total distance traveled of about 1.4 * i
                double update = 1 + Math.Abs(this.motionModel.Sample());
                distanceTraveled += update;

                // observation is remaining distance + noise
                double observation = (this.targetPosition - distanceTraveled) + this.sensorModel.Sample();

                this.Update(update, observation);
            }

            return distanceTraveled;
        }
    }
}
